/** @file md5_anim.cpp
    @brief MD5 model animations */

#include "md5/md5_object.h"

using namespace G3;

//==============================================================================
// Constructors
//==============================================================================
G3::MD5_MODEL::ANIMATION::ANIMATION () {
    numFrames = 0;
    framerate = 0;
    numJoints = 0;

    numAnimatedComponents = 0;

    log = &G3::MainLog;
}

G3::MD5_MODEL::ANIMATION::JOINT_INFO::JOINT_INFO () {
    parent = 0;
    flags = 0;
    startIndex = 0;
}

//==============================================================================
// Destructors
//==============================================================================
G3::MD5_MODEL::ANIMATION::~ANIMATION () {
    if (!skeletonFrames.empty ())
        skeletonFrames.clear ();
    if (!bboxes.empty ())
        bboxes.clear ();
    if (!jointInfos.empty ())
        jointInfos.clear ();
    if (!baseFrame.empty ())
        baseFrame.clear ();
}

G3::MD5_MODEL::ANIMATION::JOINT_INFO::~JOINT_INFO () {
    name = "";
}

//==============================================================================
// Operators
//==============================================================================
MD5_MODEL::ANIMATION::JOINT_INFO& G3::MD5_MODEL::ANIMATION::JOINT_INFO::operator= (const JOINT_INFO &ji) {
    name = ji.name;
    parent = ji.parent;
    flags = ji.flags;
    startIndex = ji.startIndex;

    return *this;
}

//==============================================================================
// Parses the animation hierarchy
//==============================================================================
bool G3::MD5_MODEL::ANIMATION::ParseHierarchy (G3::STREAM *stream) {
    Assert (stream, "MD5_MODEL::ANIMATION::ParseHierarchy: Stream argument is a NULL pointer..");

    std::string line;
    G3::TOKENLIST tokl;
    JOINT_INFO jInfo;

    // Parse the mesh scope
    while (!stream->EndOfFile ()) {
        // Read an uncommented line
        line = G3::CopyUntilDelim (stream->Readline ("\r\n"), "//");
        // Tokenize the line
        tokl = G3::Tokenize (line, " \t\"(){}");

        // There are at least 4 important tokens?
        if (tokl.Tokens.size () >= 4) {
            // Fetch joint information
            jInfo.name = tokl.Tokens [0];
            jInfo.parent = (int) G3::StrToInteger (tokl.Tokens [1]);
            jInfo.flags = (int) G3::StrToInteger (tokl.Tokens [2]);
            jInfo.startIndex = (int) G3::StrToInteger (tokl.Tokens [3]);
            // Add this joint information
            jointInfos.push_back (jInfo);
        }

        // End of scope?
        if (line.find ("}") != std::string::npos)
            break;
    }

    return true;
}

//==============================================================================
// Parses the bounding boxes
//==============================================================================
bool G3::MD5_MODEL::ANIMATION::ParseBounds (G3::STREAM *stream) {
    Assert (stream, "MD5_MODEL::ANIMATION::ParseBounds: Stream argument is a NULL pointer..");

    std::string line;
    G3::TOKENLIST tokl;
    G3::AABB3F *bbox = NULL;

    // Parse the mesh scope
    while (!stream->EndOfFile ()) {
        // Read an uncommented line
        line = G3::CopyUntilDelim (stream->Readline ("\r\n"), "//");
        // Tokenize the line
        tokl = G3::Tokenize (line, " \t\"(){}");

        // There are at least 6 important tokens?
        if (tokl.Tokens.size () >= 6) {
            // Allocate memory for another bounding box
            bbox = new G3::AABB3F ();
            // FAIL?
            if (!bbox) {
                if (log)
                    log->Report ("ERROR! MD5_MODEL::ANIMATION::ParseBounds: Failed to allocate memory for an AABB3F\n");
                return false;
            }
            // Fetch bounding box's coordinates
            bbox->Minimum.x = G3::StrToFloat (tokl.Tokens [0]);
            bbox->Minimum.y = G3::StrToFloat (tokl.Tokens [1]);
            bbox->Minimum.z = G3::StrToFloat (tokl.Tokens [2]);
            bbox->Maximum.x = G3::StrToFloat (tokl.Tokens [3]);
            bbox->Maximum.y = G3::StrToFloat (tokl.Tokens [4]);
            bbox->Maximum.z = G3::StrToFloat (tokl.Tokens [5]);
            // Add this bounding box
            bboxes.push_back (bbox);
        }

        // End of scope?
        if (line.find ("}") != std::string::npos)
            break;
    }

    return true;
}

//==============================================================================
// Parses the base frame
//==============================================================================
bool G3::MD5_MODEL::ANIMATION::ParseBaseFrame (G3::STREAM *stream) {
    Assert (stream, "MD5_MODEL::ANIMATION::ParseBaseFrame: Stream argument is a NULL pointer..");

    std::string line;
    G3::TOKENLIST tokl;
    BASEFRAME_JOINT bfJoint;

    // Parse the mesh scope
    while (!stream->EndOfFile ()) {
        // Read an uncommented line
        line = G3::CopyUntilDelim (stream->Readline ("\r\n"), "//");
        // Tokenize the line
        tokl = G3::Tokenize (line, " \t\"(){}");

        // There are at least 6 important tokens?
        if (tokl.Tokens.size () >= 6) {
            // Fetch joint position
            bfJoint.position.x = G3::StrToFloat (tokl.Tokens [0]);
            bfJoint.position.y = G3::StrToFloat (tokl.Tokens [1]);
            bfJoint.position.z = G3::StrToFloat (tokl.Tokens [2]);
            // Fetch joint orientation
            bfJoint.orientation.x = G3::StrToFloat (tokl.Tokens [3]);
            bfJoint.orientation.y = G3::StrToFloat (tokl.Tokens [4]);
            bfJoint.orientation.z = G3::StrToFloat (tokl.Tokens [5]);
            // Add this joint
            baseFrame.push_back (bfJoint);
        }

        // End of scope?
        if (line.find ("}") != std::string::npos)
            break;
    }

    return true;
}

//==============================================================================
// Parses a frame
//==============================================================================
bool G3::MD5_MODEL::ANIMATION::ParseFrame (G3::STREAM *stream) {
    Assert (stream, "MD5_MODEL::ANIMATION::ParseFrame: Stream argument is a NULL pointer..");

    std::string line;
    G3::TOKENLIST tokl;
    bool retval = false;

    // Parse the mesh scope
    while (!stream->EndOfFile ()) {
        // Read an uncommented line
        line = G3::CopyUntilDelim (stream->Readline ("\r\n"), "//");
        // Tokenize the line
        tokl = G3::Tokenize (line, " \t\"(){}");

        for (int i=0; i<(int) tokl.Tokens.size (); i++) {
            if (G3::IsNumber (tokl.Tokens [i]))
                animData.push_back (G3::StrToFloat (tokl.Tokens [i]));
        }

        // End of scope?
        if (line.find ("}") != std::string::npos)
            break;
    }
/*
    G3::MainLog.Report ("Dumping Frame..\n");

    for (int i=0; i<(int) animData.size (); i+=6)
        G3::MainLog.Report ("%f %f %f %f %f %f\n", animData [i], animData [i + 1], animData [i + 2], animData [i + 3], animData [i + 4], animData [i + 5]);
*/
    // Build a skeleton for this animation
    retval = BuildFrameSkeleton ();
    // The animation data is no longer needed
    if (!animData.empty ())
        animData.clear ();

    return retval;
}

//==============================================================================
// Parses the whole *.md5_anim file
//==============================================================================
bool G3::MD5_MODEL::ANIMATION::ParseAnimation (G3::STREAM *stream) {
    Assert (stream, "MD5_MODEL::ParseAnimation: Stream argument is a NULL pointer..");

    std::string line;
    G3::TOKENLIST tokl;

    // Parse the whole MD5Anim file
    while (!stream->EndOfFile ()) {
        // Read an uncommented line
        line = G3::CopyUntilDelim (stream->Readline ("\r\n"), "//");
        // Tokenize the line
        tokl = G3::Tokenize (line, " \t\"(){}");
        // There are at least 2 important tokens?
        if (tokl.Tokens.size () >= 2) {
            // A frame?
            if (tokl.Tokens [0] == "frame") {
                if (!ParseFrame (stream)) {
                    if (!jointInfos.empty ())
                        jointInfos.clear ();
                    if (!baseFrame.empty ())
                        baseFrame.clear ();
                    return false;
                }

                // Number of frames?
            } else if (tokl.Tokens [0] == "numFrames") {
                numFrames = (int) StrToInteger (tokl.Tokens [1]);
                // Reserve for a certain amount of frames
                skeletonFrames.reserve (numFrames);
                bboxes.reserve (numFrames);

                // Number of joints?
            } else if (tokl.Tokens [0] == "numJoints") {
                numJoints = (int) StrToInteger (tokl.Tokens [1]);
                // Reserve for a certain amount of joints
                jointInfos.reserve (numJoints);
                baseFrame.reserve (numJoints);

                // Framerate?
            } else if (tokl.Tokens [0] == "frameRate") {
                framerate = (int) StrToInteger (tokl.Tokens [1]);

                // Number of animated components?
            } else if (tokl.Tokens [0] == "numAnimatedComponents") {
                numAnimatedComponents = (int) StrToInteger (tokl.Tokens [1]);
                // Reserve for a certain amount of floats
                animData.reserve (numAnimatedComponents);
            }

            // Lets hope that everything is on its own line
        } else if (tokl.Tokens.size () == 1) {
            // Hierarchy scope?
            if (tokl.Tokens [0] == "hierarchy") {
                if (!ParseHierarchy (stream)) {
                    if (!jointInfos.empty ())
                        jointInfos.clear ();
                    if (!baseFrame.empty ())
                        baseFrame.clear ();
                    return false;
                }

                // Bounds scope?
            } else if (tokl.Tokens [0] == "bounds") {
                if (!ParseBounds (stream)) {
                    if (!jointInfos.empty ())
                        jointInfos.clear ();
                    if (!baseFrame.empty ())
                        baseFrame.clear ();
                    return false;
                }

                // Baseframe scope?
            } else if (tokl.Tokens [0] == "baseframe") {
                if (!ParseBaseFrame (stream)) {
                    if (!jointInfos.empty ())
                        jointInfos.clear ();
                    if (!baseFrame.empty ())
                        baseFrame.clear ();
                    return false;
                }
            }
        }
    }

    //Dump ("", &G3::MainLog);

    if (!jointInfos.empty ())
        jointInfos.clear ();
    if (!baseFrame.empty ())
        baseFrame.clear ();

    return true;
}

//==============================================================================
// Builds the skeleton for a frame
//==============================================================================
bool G3::MD5_MODEL::ANIMATION::BuildFrameSkeleton () {
    SKELETON *skel = new SKELETON ();
    BASEFRAME_JOINT *bfj = NULL;
    JOINT *joint = NULL, *parentJoint = NULL;
    G3::VEC3F animPos;
    G3::QUATERNIONF animOrient;
    int j;

    if (!skel) {
        if (log)
            log->Report ("ERROR! MD5_MODEL::ANIMATION::BuildFrameSkeleton: Failed to allocate memory for a skeleton..\n");
        return false;
    }

    skeletonFrames.push_back (skel);

    skel->joints.reserve (numJoints);

    for (int i=0; i<numJoints; i++) {
        bfj = &baseFrame [i];
        animPos = bfj->position;
        animOrient = bfj->orientation;

        j = 0;

        // Setup animated joint position
        if (jointInfos [i].flags & JOINT_INFO::JF_TX) {//MD5_JOINTFLAG_TX) {
            animPos.x = animData [jointInfos [i].startIndex + j];
            j++;
        }
        if (jointInfos [i].flags & JOINT_INFO::JF_TY) {//MD5_JOINTFLAG_TY) {
            animPos.y = animData [jointInfos [i].startIndex + j];
            j++;
        }
        if (jointInfos [i].flags & JOINT_INFO::JF_TZ) {//MD5_JOINTFLAG_TZ) {
            animPos.z = animData [jointInfos [i].startIndex + j];
            j++;
        }
        // Setup animated joint orientation
        if (jointInfos [i].flags & JOINT_INFO::JF_QX) {//MD5_JOINTFLAG_QX) {
            animOrient.x = animData [jointInfos [i].startIndex + j];
            j++;
        }
        if (jointInfos [i].flags & JOINT_INFO::JF_QY) {//MD5_JOINTFLAG_QY) {
            animOrient.y = animData [jointInfos [i].startIndex + j];
            j++;
        }
        if (jointInfos [i].flags & JOINT_INFO::JF_QZ) {//MD5_JOINTFLAG_QZ) {
            animOrient.z = animData [jointInfos [i].startIndex + j];
            j++;
        }

        animOrient.CalcW ();

        joint = new JOINT ();

        if (!joint) {
            if (log)
                log->Report ("ERROR! MD5_MODEL::ANIMATION::BuildFrameSkeleton: Failed to allocate memory for a new joint..\n");
            return false;
        }


        // NOTE: we assume that this joint's parent has
        // already been calculated, i.e. joint's ID should
        // never be smaller than its parent ID.
        skel->joints.push_back (joint);

        joint->parent = jointInfos [i].parent;
        joint->name = jointInfos [i].name;
        // Has parent?
        if (joint->parent < 0) {
            joint->position = animPos;
            joint->orientation = animOrient;
        } else {
            parentJoint = skel->joints [joint->parent];

            animPos = G3::ApplyQuat (animPos, parentJoint->orientation);
            joint->position = animPos + parentJoint->position;

            joint->orientation = parentJoint->orientation * animOrient;
            joint->orientation.Normalize ();
        }
    }

    return true;
}

//==============================================================================
// Interpolates the skeleton between two frames
//==============================================================================
void G3::MD5_MODEL::ANIMATION::Interpolate (int frameA, int frameB, float factor, SKELETON *skel) {
    Assert (skel, "ERROR! MD5_MODEL::ANIMATION::Interpolate: Skeleton argument is a NULL pointer..");
    Assert (frameA >= 0, "ERROR! MD5_MODEL::ANIMATION::Interpolate: frameA argument is invalid..");
    Assert (frameB < numFrames, "ERROR! MD5_MODEL::ANIMATION::Interpolate: frameB argument is invalid..");

    JOINT *aJoint = NULL, *bJoint = NULL, *fJoint = NULL;

    for (int i=0; i<(int) skel->joints.size (); i++) {
        aJoint = skeletonFrames [frameA]->joints [i];
        bJoint = skeletonFrames [frameB]->joints [i];
        fJoint = skel->joints [i];

        fJoint->parent = aJoint->parent;
        fJoint->position = aJoint->position + (bJoint->position - aJoint->position)  * factor;
        fJoint->orientation = G3::Slerp (aJoint->orientation, bJoint->orientation, factor);
    }
}

//==============================================================================
// Dumps the JointInfo into the log
//==============================================================================
void G3::MD5_MODEL::ANIMATION::JOINT_INFO::Dump (std::string indent, G3::LOG *log) {
    if (log) {
        log->Report ("%sDumping JointInfo \"%s\":\n"
                     "%s Parent: %d\n"
                     "%s Flags: %d\n"
                     "%s StartIndex: %d\n",
                     indent.c_str (), name.c_str (),
                     indent.c_str (), parent,
                     indent.c_str (), flags,
                     indent.c_str (), startIndex);
    }
}

//==============================================================================
// Dumps the animation into the log
//==============================================================================
void G3::MD5_MODEL::ANIMATION::Dump (std::string indent, G3::LOG *log) {
    if (log) {
        log->Report ("%sDumping Animation \"%s\":\n"
                     "%s NumFrames: %d\n"
                     "%s Framerate: %d\n"
                     "%s NumAnimatedComponents: %d\n"
                     "%s NumJoints: %d\n",
                     indent.c_str (), name.c_str (),
                     indent.c_str (), numFrames,
                     indent.c_str (), framerate,
                     indent.c_str (), numAnimatedComponents,
                     indent.c_str (), numJoints);

        log->Report ("%s Dumping BaseFrame:\n", indent.c_str ());
        for (int i=0; i<numJoints; i++) {
            baseFrame [i].Dump (indent + "  ", log);
        }

        log->Report ("%s Dumping JointInfos:\n", indent.c_str ());
        for (int i=0; i<numJoints; i++) {
            jointInfos [i].Dump (indent + "  ", log);
        }

        log->Report ("%s Dumping SkeletonFrames:\n", indent.c_str ());

        for (int i=0; i<numFrames; i++) {
            skeletonFrames [i]->Dump (indent + "  ", log);
        }
    }
}
